#include "RobotIdentification.h"
#include "../nxColor.h"
#include "../cmvision/cmvision.h"
#include "Robot.h"
#include <cv.h>
#include <math.h>
#include <stdio.h>
RobotIdentification::RobotIdentification(Robot** robotArray, int robotsNumber, CMVision* cmvision)
{
	
	/*FIRST STEP
	 * Take the values of colors in cmvision and convert it to format int -> 0x00RRGGBB
	 * and save it in a buffer, colors.
	*/
	this->cmvision = cmvision;
	updateColors();
	
	/*SECOND STEP
	 *  
	 * 
	*/
	this->robotArray = robotArray;
	this->robotsNumber = robotsNumber;	

}



RobotIdentification::~RobotIdentification()
{

}

Robot** RobotIdentification::getRobots()
{

}

/**********************************
 * GETROBOT
 *********************************/
Robot* RobotIdentification::getRobot(int index)
{
	//recover robot from array
	Robot* robot = robotArray[index];
	
	//get colors of robot
	nxColor* baseColor = robot->getBaseColor();
	nxColor* robotColor = robot->getRobotColor();
	
	
	//get index of colors from cmvision
	int indexBaseColor = getColorIndex(baseColor->getMeanValueInt(RGB_CODE));
	int indexRobotColor = getColorIndex(robotColor->getMeanValueInt(RGB_CODE));
	
	//printf("index base: %d, index robot: %d\n",indexBaseColor,indexRobotColor);
	//detected regions
	CMVision::region* regionRobot = NULL;
	
	//get min and max area for blobs
	int minArea = robot->getMinArea();
	int maxArea = robot->getMaxArea();
	
	//get min and max Dist between blobs
	int minDist = robot->getMinDist();
	int maxDist = robot->getMaxDist();
	
	
	//printf("minArea: %d maxArea %d\n",minArea,maxArea);
	//printf("minDist: %d maxDist %d\n",minDist,maxDist);
	
	//First Region of robot Color. It is supposed to be the largest one, then, we use it.
	regionRobot = cmvision->getRegions(indexRobotColor);

	for(regionRobot = cmvision->getRegions(indexRobotColor); regionRobot!=NULL;regionRobot = regionRobot->next)
	{
		int area   = (regionRobot->area);
		//if region complies with min and max area, then break loop	
		if(area>=minArea && area<=maxArea)
			break;
	
	}
	
	//if no region found, there is no robot!!
	if(regionRobot ==NULL)
	{
		printf("Error in RegionRobot (RobotIdentification)\n");
		return NULL;
	}
	//get the nearest base region
	CMVision::region* regionBase = getNearestRegion(regionRobot,indexBaseColor, minArea, maxArea, minDist,maxDist);
	
	//position of center of first region
	float x2 = regionRobot->cen_x;
	float y2 = regionRobot->cen_y;
	
	//set position of robot region
	robot->setRobotColorPos(cvPoint((int)x2,(int)y2));
	
	//if no region found, there is no robot!!
	if(regionBase ==NULL)
	{
		printf("Error in RegionBase (RobotIdentification)\n");
		return NULL;
	}
	//position of center of second region
	float x1      = regionBase->cen_x;
    float y1      = regionBase->cen_y;
    
    //set position of base region
    robot->setBaseColorPos(cvPoint((int)x1,(int)y1));
    
    
    
    return robot;
	
}
/**********************************
 * GETNEARESTREGION
 *********************************/
CMVision::region* RobotIdentification::getNearestRegion(CMVision::region* regionRobot, int indexColor, int minArea, int maxArea, int minDist, int maxDist)
{
	CMVision::region* regionBase = NULL;
	CMVision::region* regionResult = NULL;
	//position of center of region
	float x2 = regionRobot->cen_x;
	float y2 = regionRobot->cen_y;
	
	
	int i=0;
	float lastDist;
	bool first = false;
	for (regionBase = cmvision->getRegions(indexColor); (regionBase!=NULL); regionBase = regionBase->next)
	{
		int area = regionBase->area;
		
		//only pay attention to blobs that applies with min and max area
		if(area>=minArea && area<=maxArea)
		{
	        float x1      = regionBase->cen_x;
	        float y1      = regionBase->cen_y;
	        
	        float difX = x2 -x1;
	        float difY = y2 -y1;
	        //calculate distance
	        float dist = difX*difX + difY*difY;
	        
	        if(minDist<maxDist && minDist>0)
	        {
	        	if(dist>=minDist*minDist && dist<=maxDist*maxDist)
	        	{
	        		regionResult = regionBase;
	        		break;
	        	}
	        
	        }
	        //save first distance in the variable to be compared with the others
	        if (!first)
	        {
	        	lastDist = dist;
	        	regionResult = regionBase;
	        	first = true;
	        	
	        }
	        // the other distances will be compared with the shortest one.
	        else if(dist<lastDist){
	        	lastDist = dist;
	        	regionResult = regionBase;
	        	
	        }
		}
        i++;
      }
      return regionResult;
}
/**********************************
 * GETCOLORINDEX
 *********************************/

int RobotIdentification::getColorIndex(int color)
{
	for(int i=0;i<CMV_MAX_COLORS;i++)
		if(colors[i] == color)
			return i;
	return -1;
}

void RobotIdentification::updateColors()
{
	//colorsNumber = cmvision->getColorsNumber();
	colors = new int[CMV_MAX_COLORS];
	//find values of implemented colors in cmvision
	for(int i=0;i<CMV_MAX_COLORS;i++){
		//get info
		CMVision::color_info* colorInfo = cmvision->getColorInfo(i);
		rgb colorData = colorInfo->color;
		CvScalar cd = cvScalar(colorData.red, colorData.green, colorData.blue);
		//convert rgb value from structure to int, in the form of 0x00RRGGBB, as implemented in Player
		int rgbValue = nxColor::scalar2int(cd);
		colors[i] = rgbValue;
		
	}
}

